//My C++ incdlues
#include <iostream>
#include <cmath>

//My own headers
#include "server.h"
#include "polygon.h"

// #define DEBUG_SUMMARY

pthread_mutex_t data_mutex;

const double field_of_view = 68.5;              //total field of view
const double angle_per_pixel = 68.5 / 640.0;    //angle per pixel (field of view / total pixels)
const double focal_dist = 745.78;               //focal distance

const double obj_size_h = 62;   //width of a target
const double obj_size_v1 = 20;  //height of top target
const double obj_size_v2 = 29;  //height of middle targets

directions direc;

int getShooterSpeed(double distance);
void processFrame(Mat &image, RectangleScanner&scanner)
{
    RotatedRect rectangle;
    directions new_direc;
   
#ifdef SAVE_IMAGES
    saveFrame((char*)"raw_", &image);
#endif /* SAVE_IMAGES */
    scanner.SetImage(image);
    scanner.ChooseRectangle();
    
#ifdef SAVE_IMAGES
    if (scanner.top_rect_selected) {
        saveFrame((char *)"final_", &(scanner.src_rects));
    } else {
        saveFrame((char *)"nothing_", &(scanner.src_rects));
    }
#endif /* SAVE_IMAGES */

    if (scanner.top_rect_selected) {
#ifdef DEBUG_SUMMARY
        cout << "Chosen rectangle info:" << endl;
        cout << "Width:  " << scanner.best_match_rect.size.width << endl;
        cout << "Height: " << scanner.best_match_rect.size.height << endl;
        cout << "Center X: " << scanner.best_match_rect.center.x << endl;
        cout << "Center Y: " << scanner.best_match_rect.center.y << endl;
        cout << "X Offset From Center " << scanner.best_match_rect.center.x - 320 << endl;
        cout << "Y Offset From Center " << scanner.best_match_rect.center.y - 240 << endl;
#endif /* DEBUG_SUMMARY */
        
#if 1

        new_direc.dist = obj_size_v1 * focal_dist / scanner.best_match_rect.size.height;
#else
        // Victor's home small object right height
        new_direc.dist = obj_size_v1 * focal_dist / scanner.best_match_rect.size.height;
#endif /* 0 */

        new_direc.shooter_speed = getShooterSpeed(new_direc.dist);
        new_direc.horiz_angle = (scanner.best_match_rect.center.x - 320) * angle_per_pixel;
        new_direc.vert_angle = (scanner.best_match_rect.center.y - 240) * angle_per_pixel;
        
        new_direc.status = 0;

#ifdef DEBUG_SUMMARY
        cout << "Distance is " << new_direc.dist << endl;
        cout << "Shooter speed is " << new_direc.shooter_speed << endl;
        cout << "Horizontal angle is " << new_direc.horiz_angle << endl;
        cout << "Vertical angle is " << new_direc.vert_angle << endl;
        cout << "Status is " << new_direc.status << endl << endl;
#endif /* DEBUG_SUMMARY */
    } else {
        new_direc.status = 5;
#ifdef DEBUG_SUMMARY
        cout << "Nothing selected: Status is " << new_direc.status << endl;
#endif /* DEBUG_SUMMARY */
    }
    pthread_mutex_lock(&data_mutex); //Lock structure direc
    direc = new_direc;
    pthread_mutex_unlock(&data_mutex); //Unlock structure direc
}   

struct distSpeed {
    double distance;
    double speed;
};

struct distSpeed table[] = {
    {235, 0.68},
    {260, 0.69}
};

int getShooterSpeed(double distance) {

    double ret = 0;
    int i = 0;
    int len = sizeof(table) / sizeof(struct distSpeed);

    for ( ; i < len ; i++ ) {
        if (distance > table[i].distance) {
            break;
        }
    }

    if ( i < len - 1) {
        ret = table[i].speed + ((table[i + 1].speed - table[i].speed)) * (distance - table[i].distance) / (table[i + 1].distance - table[i].distance);
    }
    else {
        ret = table[len-1].speed;
    }
    return (int) (ret * 1000);
    
}
